#include "constants.h"
#include "library.h"

#ifndef MYPOSE_H_
#define MYPOSE_H_

/**
 * Pose 2D message has only x,y,theta
 * whereas we'll need to store the transform of each pose in the odom frame to the map frame
 */

class MyPose : geometry_msgs::Pose2D	{
private:
	double odomX, odomY, odomTheta;
	tf::Transform map_to_odom;
public:
	MyPose();
	virtual ~MyPose();
	bool setPoseInMap(double x,double y,double theta,tf::TransformListener &listener);
	bool updatePoseInOdom(double x,double y, double theta);
	bool getPoseInOdom(double &x, double &y, double &theta);
	bool getPoseInMap(double &x, double &y, double &theta) const;
	bool isClose(DDD &pose);
	tf::Transform getMapOdomTransform();
};

typedef std::vector<MyPose> PoseArray;

#endif /* MYPOSE_H_ */
